/*
Robotics object oriented package in C++
Copyright (C) 2008-2009  Matrix

This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation; either version 2.1 of the
License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston,  MA 02111-1307  USA
*/
#include "StdAfx.h"
#include "Rotation3.h"

namespace RobotPlant { namespace Math{
	Rotation3::Rotation3(void)
	{
		_datas[0][0] = 1; _datas[0][1] = 0; _datas[0][2] = 0;
		_datas[1][0] = 0; _datas[1][1] = 1; _datas[1][2] = 0;
		_datas[2][0] = 0; _datas[2][1] = 0; _datas[2][2] = 1;
	}

	Rotation3::Rotation3(   const double Xx,const double Yx,const double Zx,
							const double Xy,const double Yy,const double Zy,
							const double Xz,const double Yz,const double Zz)
	{
		_datas[0][0] = Xx; _datas[0][1] = Yx; _datas[0][2] = Zx;
		_datas[1][0] = Xy; _datas[1][1] = Yy; _datas[1][2] = Zy;
		_datas[2][0] = Xz; _datas[2][1] = Yz; _datas[2][2] = Zz;
	}

	Rotation3::Rotation3(const Vector3& x,const Vector3& y,const Vector3& z) 
	{
		_datas[0][0] = x[0]; _datas[0][1] = y[0]; _datas[0][2] = z[0];
		_datas[1][0] = x[1]; _datas[1][1] = y[1]; _datas[1][2] = z[1];
		_datas[2][0] = x[2]; _datas[2][1] = y[2]; _datas[2][2] = z[2];
	}

	Rotation3::Rotation3(const Quaternion& quat)
	{
		*this = quat.Rotation();
	}

	Rotation3::~Rotation3(void)
	{
	}

	double& Rotation3::operator () (const int row,const int column)
	{
		if((row < 0) || (row > 2))
			throw(new std::exception("Argument row index must be between 0 and 2."));
		if((column < 0) || (column > 2))
			throw(new std::exception("Argument column index must be between 0 and 2."));

		return _datas[row][column];
	}

	double Rotation3::operator () (const int row,const int column) const
	{
		if((row < 0) || (row > 2))
			throw(new std::exception("Argument row index must be between 0 and 2."));
		if((column < 0) || (column > 2))
			throw(new std::exception("Argument column index must be between 0 and 2."));

		return _datas[row][column];
	}

	Rotation3& Rotation3::operator = (const Rotation3& arg)
	{
		for (int row = 0; row < 3; row++)
		{
			for (int column = 0; column < 3; column++)
			{
				_datas[row][column] = arg._datas[row][column];
			}
		}
		return *this;
	}

	bool operator == (const Rotation3& lhs, const Rotation3& rhs) 
	{
		return Equal(lhs,rhs);
	}


	bool operator != (const Rotation3& lhs, const Rotation3& rhs)
	{
		return !Equal(lhs,rhs);
	}

	Vector3 operator * (const Rotation3& lhs, const Vector3& rhs)
	{
		// Complexity : 9M+6A
		return Vector3(
			lhs(0,0)*rhs[0] + lhs(0,1)*rhs[1] + lhs(0,2)*rhs[2],
			lhs(1,0)*rhs[0] + lhs(1,1)*rhs[1] + lhs(1,2)*rhs[2],
			lhs(2,0)*rhs[0] + lhs(2,1)*rhs[1] + lhs(2,2)*rhs[2]);
	}

	Velocity operator * (const Rotation3& lhs, const Velocity& rhs)
	{
		// Transformation of the base to which the twist is expressed.
		// look at Frame*Velocity for a transformation that also transforms
		// the velocity reference point.
		// Complexity : 18M+12A
		return Velocity(lhs * rhs.LinearVel,lhs * rhs.AngleVel);
	}

	Wrench operator * (const Rotation3& lhs, const Wrench& rhs)
	{
		// Transformation of the base to which the wrench is expressed.
		// look at Frame*Velocity for a transformation that also transforms
		// the Force reference point.
		return Wrench(lhs * rhs.Force,lhs * rhs.Moment);
	}

	Rotation3 operator * (const Rotation3& lhs, const Rotation3& rhs)
	{
		// Complexity : 27M+27A
		return Rotation3(
			lhs._datas[0][0] * rhs._datas[0][0] + lhs._datas[0][1] * rhs._datas[1][0] + lhs._datas[0][2] * rhs._datas[2][0] ,
			lhs._datas[0][0] * rhs._datas[0][1] + lhs._datas[0][1] * rhs._datas[1][1] + lhs._datas[0][2] * rhs._datas[2][1] ,
			lhs._datas[0][0] * rhs._datas[0][2] + lhs._datas[0][1] * rhs._datas[1][2] + lhs._datas[0][2] * rhs._datas[2][2] ,
			lhs._datas[1][0] * rhs._datas[0][0] + lhs._datas[1][1] * rhs._datas[1][0] + lhs._datas[1][2] * rhs._datas[2][0] ,
			lhs._datas[1][0] * rhs._datas[0][1] + lhs._datas[1][1] * rhs._datas[1][1] + lhs._datas[1][2] * rhs._datas[2][1] ,
			lhs._datas[1][0] * rhs._datas[0][2] + lhs._datas[1][1] * rhs._datas[1][2] + lhs._datas[1][2] * rhs._datas[2][2] ,
			lhs._datas[2][0] * rhs._datas[0][0] + lhs._datas[2][1] * rhs._datas[1][0] + lhs._datas[2][2] * rhs._datas[2][0] ,
			lhs._datas[2][0] * rhs._datas[0][1] + lhs._datas[2][1] * rhs._datas[1][1] + lhs._datas[2][2] * rhs._datas[2][1] ,
			lhs._datas[2][0] * rhs._datas[0][2] + lhs._datas[2][1] * rhs._datas[1][2] + lhs._datas[2][2] * rhs._datas[2][2] 
		);
	}

	void Rotation3::SetInverse()
	{
		double tmp;

		tmp = _datas[0][1];
		_datas[0][1] = _datas[1][0];
		_datas[1][0] = tmp;

		tmp = _datas[0][2];
		_datas[0][2] = _datas[2][0];
		_datas[2][0] = tmp;

		tmp = _datas[1][2];
		_datas[1][2] = _datas[2][1];
		_datas[2][1] = tmp;
	}

	Rotation3 Rotation3::Inverse() const
	{
		Rotation3 tmp(*this);
		tmp.SetInverse();
		return tmp;
	}

	Vector3 Rotation3::Inverse(const Vector3& v) const 
	{
		return Vector3(
			_datas[0][0] * v[0] + _datas[1][0] * v[1] + _datas[2][0] * v[2],
			_datas[0][1] * v[0] + _datas[1][1] * v[1] + _datas[2][1] * v[2],
			_datas[0][2] * v[0] + _datas[1][2] * v[1] + _datas[2][2] * v[2] 
		);
	}


	Wrench Rotation3::Inverse(const Wrench& arg) const
	{
		return Wrench(Inverse(arg.Force),Inverse(arg.Moment));
	}

	Velocity Rotation3::Inverse(const Velocity& arg) const
	{
		return Velocity(Inverse(arg.LinearVel),Inverse(arg.AngleVel));
	}

	// *this = *this * ROT(X,angle)
	void Rotation3::DoRotX(const double angle)
	{
		double cs = cos(angle);
		double sn = sin(angle);
		double x1,x2,x3;
		x1  = cs* (*this)(0,1) + sn* (*this)(0,2);
		x2  = cs* (*this)(1,1) + sn* (*this)(1,2);
		x3  = cs* (*this)(2,1) + sn* (*this)(2,2);
		(*this)(0,2) = -sn* (*this)(0,1) + cs* (*this)(0,2);
		(*this)(1,2) = -sn* (*this)(1,1) + cs* (*this)(1,2);
		(*this)(2,2) = -sn* (*this)(2,1) + cs* (*this)(2,2);
		(*this)(0,1) = x1;
		(*this)(1,1) = x2;
		(*this)(2,1) = x3;
	}

	void Rotation3::DoRotY(const double angle)
	{
		double cs = cos(angle);
		double sn = sin(angle);
		double x1,x2,x3;
		x1  = cs* (*this)(0,0) - sn* (*this)(0,2);
		x2  = cs* (*this)(1,0) - sn* (*this)(1,2);
		x3  = cs* (*this)(2,0) - sn* (*this)(2,2);
		(*this)(0,2) = sn* (*this)(0,0) + cs* (*this)(0,2);
		(*this)(1,2) = sn* (*this)(1,0) + cs* (*this)(1,2);
		(*this)(2,2) = sn* (*this)(2,0) + cs* (*this)(2,2);
		(*this)(0,0) = x1;
		(*this)(1,0) = x2;
		(*this)(2,0) = x3;
	}

	void Rotation3::DoRotZ(const double angle)
	{
		double cs = cos(angle);
		double sn = sin(angle);
		double x1,x2,x3;
		x1  = cs* (*this)(0,0) + sn* (*this)(0,1);
		x2  = cs* (*this)(1,0) + sn* (*this)(1,1);
		x3  = cs* (*this)(2,0) + sn* (*this)(2,1);
		(*this)(0,1) = -sn* (*this)(0,0) + cs* (*this)(0,1);
		(*this)(1,1) = -sn* (*this)(1,0) + cs* (*this)(1,1);
		(*this)(2,1) = -sn* (*this)(2,0) + cs* (*this)(2,1);
		(*this)(0,0) = x1;
		(*this)(1,0) = x2;
		(*this)(2,0) = x3;
	}

	Vector3 Rotation3::GetRot() const
	{
		// Returns a vector with the direction of the equiv. axis
		// and its norm is angle
		Vector3 axis  = Vector3((_datas[2][1] -_datas[1][2]),
								(_datas[0][2]-_datas[2][0]),
								(_datas[1][0]-_datas[0][1]) )/2;

		double sa    = axis.Norm();
		double ca    = (_datas[0][0]+_datas[1][1]+_datas[2][2]-1)/2.0;
		double alfa;
		if (sa > Epsilon)
			alfa = atan2(sa,ca)/sa;
		else
			alfa = 1;
		return axis * alfa;
	}

	/** Returns the rotation angle around the equiv. axis
	* @param axis the rotation axis is returned in this variable
	* @param eps :  in the case of angle == 0 : Rot axis is undefined and choosen
	*                                         to be +/- Z-axis
	*               in the case of angle == PI : 2 solutions, positive Z-component
	*                                            of the axis is choosen.
	* @result returns the rotation angle (between [0..PI] )
	* /todo :
	*   Check corresponding routines in rframes and rrframes
	*/
	void Rotation3::GetRotAngle(Vector3& axis, double& angle, const double eps) const
	{
		double ca    = (_datas[0][0] + _datas[1][1] + _datas[2][2] -1)/2.0;
		if (ca>1-eps) 
		{
			// undefined choose the Z-axis, and angle 0
			axis = Vector3(0,0,1);
			angle = 0;
		}
		else if (ca < -1+eps) 
		{
			// two solutions, choose a positive Z-component of the axis
			double z = sqrt( (_datas[2][2] + 1)/2 );
			double x = (_datas[0][2])/2/z;
			double y = (_datas[1][2])/2/z;
			axis = Vector3( x,y,z  );
			angle = PI;
		}
		else
		{
			angle = acos(ca);
			double sa    = sin(angle);
			axis  = Vector3((_datas[2][1] - _datas[1][2])/2/sa,
				(_datas[0][2] - _datas[2][0])/2/sa,
				(_datas[1][0] - _datas[0][1])/2/sa  );
		}
	}

	// Gives back a rotation matrix specified with RPY convention
	void Rotation3::GetRPY(double& roll,double& pitch,double& yaw) const
	{
		if (fabs(_datas[2][0]) > 1.0 - Epsilon )
		{
			roll = -sign(_datas[2][0]) * atan2(_datas[0][1], _datas[1][1]);
			pitch= -sign(_datas[2][0]) * PI / 2;
			yaw  = 0.0 ;
		} 
		else
		{
			roll  = atan2(_datas[2][1], _datas[2][2]);
			pitch = atan2(-_datas[2][0], sqrt( sqr(_datas[0][0]) +sqr(_datas[1][0]) )  );
			yaw   = atan2(_datas[1][0], _datas[0][0]);
		}
	}

	void Rotation3::GetEulerZYZ(double& alfa,double& beta,double& gamma) const
	{
		if (fabs(_datas[2][0]) < Epsilon )
		{
			alfa=0.0;
			if (_datas[2][2]>0)
			{
				beta = 0.0;
				gamma= atan2(-_datas[0][1],_datas[0][0]);
			} 
			else
			{
				beta = PI;
				gamma= atan2(_datas[0][1],-_datas[0][0]);
			}
		} 
		else
		{
			alfa=atan2(_datas[1][2], _datas[0][2]);
			beta=atan2(sqrt( sqr(_datas[2][0]) +sqr(_datas[2][1]) ),_datas[2][2]);
			gamma=atan2(_datas[2][1], -_datas[2][0]);
		}
	}

	void Rotation3::GetEulerZYX(double& Alfa,double& Beta,double& Gamma) const
	{
		GetRPY(Gamma,Beta,Alfa);
	}

	//! Access to the underlying unit vectors of the rotation matrix
	Vector3 Rotation3::GetAxisX() const
	{
		return Vector3(_datas[0][0],_datas[1][0],_datas[2][0]);
	}

	//! Access to the underlying unit vectors of the rotation matrix
	void Rotation3::GetAxisX(Vector3& X)
	{
		X[0] = _datas[0][0];
		X[1] = _datas[1][0];
		X[2] = _datas[2][0];
	}

	//! Access to the underlying unit vectors of the rotation matrix
	Vector3 Rotation3::GetAxisY() const
	{
		return Vector3(_datas[0][1],_datas[1][1],_datas[2][1]);
	}

	//! Access to the underlying unit vectors of the rotation matrix
	void Rotation3::GetAxisY(Vector3& Y)
	{
		Y[0] = _datas[0][1];
		Y[1] = _datas[1][1];
		Y[2] = _datas[2][1];
	}

	//! Access to the underlying unit vectors of the rotation matrix
	Vector3 Rotation3::GetAxisZ() const 
	{
		return Vector3(_datas[0][2],_datas[1][2],_datas[2][2]);
	}

	//! Access to the underlying unit vectors of the rotation matrix
	void Rotation3::GetAxisZ(Vector3& Z) 
	{
		Z[0] = _datas[0][2];
		Z[1] = _datas[1][2];
		Z[2] = _datas[2][2];
	}

	Rotation3 Rotation3::Identity()
	{
		return Rotation3(   1,0,0,
							0,1,0,
							0,0,1);
	}

	Rotation3 Rotation3::EulerZYX(const double Alfa,const double Beta,const double Gamma)
	{
		return RPY(Gamma,Beta,Alfa);
	}

	Rotation3 Rotation3::EulerZYZ(const double Alfa,const double Beta,const double Gamma)
	{
		double sa,ca,sb,cb,sg,cg;
		sa  = sin(Alfa);ca = cos(Alfa);
		sb  = sin(Beta);cb = cos(Beta);
		sg  = sin(Gamma);cg = cos(Gamma);
		return Rotation3( ca*cb*cg-sa*sg,     -ca*cb*sg-sa*cg,        ca*sb,
			sa*cb*cg+ca*sg,     -sa*cb*sg+ca*cg,        sa*sb,
			-sb*cg ,                sb*sg,              cb
			);

	}


	Rotation3 Rotation3::RPY(const double roll,const double pitch,const double yaw)
	{
		double ca1,cb1,cc1,sa1,sb1,sc1;
		ca1 = cos(yaw); sa1 = sin(yaw);
		cb1 = cos(pitch);sb1 = sin(pitch);
		cc1 = cos(roll);sc1 = sin(roll);
		return Rotation3(ca1*cb1,ca1*sb1*sc1 - sa1*cc1,ca1*sb1*cc1 + sa1*sc1,
			sa1*cb1,sa1*sb1*sc1 + ca1*cc1,sa1*sb1*cc1 - ca1*sc1,
			-sb1,cb1*sc1,cb1*cc1);
	}

	//Rotation3 Rotation3::Rot(const Vector3& rotaxis,const double angle)
	//{
	//	// The formula is
	//	// V.(V.tr) + st*[V x] + ct*(I-V.(V.tr))
	//	// can be found by multiplying it with an arbitrary vector p
	//	// and noting that this vector is rotated.
	//	double ct = cos(angle);
	//	double st = sin(angle);
	//	double vt = 1-ct;
	//	Vector3 rotvec = rotaxis;
	//	rotvec.Normalize();
	//	return Rotation3(
	//		ct            +  vt*rotvec[0]*rotvec[0], -rotvec[2]*st +  vt*rotvec[0]*rotvec[1], rotvec[1]*st  +  vt*rotvec[0]*rotvec[2],
	//		rotvec[2]*st  +  vt*rotvec[1]*rotvec[0], ct            +  vt*rotvec[1]*rotvec[1], -rotvec[0]*st +  vt*rotvec[1]*rotvec[2],
	//		-rotvec[1]*st +  vt*rotvec[2]*rotvec[0], rotvec[0]*st  +  vt*rotvec[2]*rotvec[1], ct            +  vt*rotvec[2]*rotvec[2]
	//		);
	//}

	Rotation3 Rotation3::Rot( const Vector3& rotvec,const double angle)
	{
		Vector3 rotVector(rotvec);
		rotVector.Normalize();

		double ct = cos(angle);
		double st = sin(angle);
		double vt = 1-ct;
		return Rotation3(
			ct            +  vt*rotVector[0]*rotVector[0], -rotVector[2]*st +  vt*rotVector[0]*rotVector[1], rotVector[1]*st  +  vt*rotVector[0]*rotVector[2],
			rotVector[2]*st  +  vt*rotVector[1]*rotVector[0], ct            +  vt*rotVector[1]*rotVector[1], -rotVector[0]*st +  vt*rotVector[1]*rotVector[2],
			-rotVector[1]*st +  vt*rotVector[2]*rotVector[0], rotVector[0]*st  +  vt*rotVector[2]*rotVector[1], ct            +  vt*rotVector[2]*rotVector[2]
			);
	}



	Rotation3 Rotation3::RotX(const double angle)
	{
		double cs=cos(angle);
		double sn=sin(angle);
		return Rotation3(1,0,0,0,cs,-sn,0,sn,cs);
	}

	Rotation3 Rotation3::RotY(const double angle)
	{
		double cs=cos(angle);
		double sn=sin(angle);
		return Rotation3(cs,0,sn,0,1,0,-sn,0,cs);
	}

	Rotation3 Rotation3::RotZ(const double angle)
	{
		double cs=cos(angle);
		double sn=sin(angle);
		return Rotation3(cs,-sn,0,sn,cs,0,0,0,1);
	}

	bool Equal(const Rotation3& a,const Rotation3& b,const double eps) 
	{
		return (Equal(a._datas[0][0],b._datas[0][0],eps) &&
			Equal(a._datas[0][1],b._datas[0][1],eps) &&
			Equal(a._datas[0][2],b._datas[0][2],eps) &&
			Equal(a._datas[1][0],b._datas[1][0],eps) &&
			Equal(a._datas[1][1],b._datas[1][1],eps) &&
			Equal(a._datas[1][2],b._datas[1][2],eps) &&
			Equal(a._datas[2][0],b._datas[2][0],eps) &&
			Equal(a._datas[2][1],b._datas[2][1],eps) &&
			Equal(a._datas[2][2],b._datas[2][2],eps)    );
	}

	Rotation3 AddDelta(const Rotation3& a,const Vector3& angleVel,const double dt)
	{
		return a * Rotation3::Rot( a.Inverse( angleVel ), angleVel.Norm() * dt );
	}

	Vector3 Diff(const Rotation3& R_a_b1,const Rotation3& R_a_b2,const double dt)
	{
		Rotation3 R_b1_b2( R_a_b1.Inverse() * R_a_b2 );
		return R_a_b1 * R_b1_b2.GetRot() / dt;
	}

	void Random(Rotation3& R) 
	{
		double alfa;
		double beta;
		double gamma;
		Random(alfa);
		Random(beta);
		Random(gamma);
		R = Rotation3::EulerZYX(alfa,beta,gamma);
	}
	void PosRandom(Rotation3& R) 
	{
		double alfa;
		double beta;
		double gamma;
		PosRandom(alfa);
		PosRandom(beta);
		PosRandom(gamma);
		R = Rotation3::EulerZYX(alfa,beta,gamma);
	}
}}
